package trackexplorer;

import java.io.BufferedReader;
import java.io.File;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.Reader;
import java.util.LinkedHashMap;
import java.util.Map;

import javax.xml.parsers.DocumentBuilder;
import javax.xml.parsers.DocumentBuilderFactory;

import org.eclipse.core.runtime.FileLocator;
import org.eclipse.core.runtime.Path;
import org.eclipse.swt.graphics.Point;
import org.eclipse.swt.graphics.RGB;
import org.w3c.dom.Document;
import org.w3c.dom.Element;
import org.w3c.dom.NodeList;

import Jama.Matrix;

public class Calibration{

    private Matrix      macierz;
    private String      datum  = null;
    private double      dz;
    private double      dy;
    private double      dx;
    private double      flatt;
    private double      a;
    private Matrix      macierz1;
    private Point       size;
    public static RGB[] colors = new RGB[]{new RGB(0, 255, 0),
        new RGB(0, 0, 255), new RGB(128, 0, 0), new RGB(255, 255, 0),
        new RGB(255, 0, 255), new RGB(0, 255, 255), new RGB(255, 127, 0),
        new RGB(255, 0, 127), new RGB(0, 255, 127), new RGB(127, 255, 0),
        new RGB(127, 0, 255), new RGB(0, 127, 255)};

    public static boolean isGraphics(File f){
        return isGraphics(f.getName());
    }

    public static boolean isGraphics(String name){
        String tmp = name.toLowerCase();
        return tmp.endsWith(".jpg") || tmp.endsWith(".jpeg")
            || tmp.endsWith(".png") || tmp.endsWith(".bmp");
    }

    public int[] geo2pix(double lat, double lon){
        Matrix geo = new Matrix(3, 1);
        geo.set(0, 0, lon);
        geo.set(1, 0, lat);
        geo.set(2, 0, 1);
        Matrix m = macierz.times(geo);
        return new int[]{(int)Math.round(m.get(0, 0)),
            (int)Math.round(m.get(1, 0))};
    }

    public double[] pix2geo(int x, int y){
        Matrix pix = new Matrix(3, 1);
        pix.set(0, 0, x);
        pix.set(1, 0, y);
        pix.set(2, 0, 1);
        return macierz1.times(pix).getColumnPackedCopy();
    }

    public void readMap(InputStream file) throws IOException{
        InputStreamReader inR = new InputStreamReader(file);
        BufferedReader r = new BufferedReader(inR);

        BufferedReader d = new BufferedReader(r);
        double[][] geo = new double[2][3];
        double[][] pix = new double[2][3];
        for(int i = 0; i < 4; i++){
            d.readLine();
        }
        String dLine = d.readLine().split(",")[0].trim();
        setDatum(dLine);
        for(int i = 0; i < 4; i++){
            d.readLine();
        }
        for(int i = 0; i < 3; i++){
            String[] split = d.readLine().split(",");
            if("".equals(split[2].trim())){
                break;
            }
            pix[0][i] = Integer.valueOf(split[2].trim());
            pix[1][i] = Integer.valueOf(split[3].trim());
            Double lat = Double.valueOf(split[6].trim());
            lat += Double.valueOf(split[7].trim()) / 60.;
            if("S".equals(split[8].trim())){
                lat *= -1;
            }

            Double lon = Double.valueOf(split[9].trim());
            lon += Double.valueOf(split[10].trim()) / 60.;
            if("W".equals(split[11].trim())){
                lon *= -1;
            }

            if(datum != null){
                Double[] tr = transform(lat, lon);
                lon = tr[0];
                lat = tr[1];
            }
            geo[0][i] = lon;
            geo[1][i] = lat;
        }
        String line;
        while((line = d.readLine()) != null){
            if(line.startsWith("IWH,")){
                int width = Integer.parseInt(line.split(",")[2]);
                int height = Integer.parseInt(line.split(",")[3]);
                size = new Point(width, height);
                break;
            }
        }
        double[][] m = new double[3][];
        m[0] = geo[0].clone();
        m[1] = geo[1].clone();
        m[2] = new double[]{1, 1, 1};
        macierz = new Matrix(pix).times(new Matrix(m).inverse());
        m = new double[3][];
        m[0] = pix[0].clone();
        m[1] = pix[1].clone();
        m[2] = new double[]{1, 1, 1};
        macierz1 = new Matrix(geo).times(new Matrix(m).inverse());
    }

    public Map<int[], String> readGpx(File file){
        LinkedHashMap<int[], String> ret = new LinkedHashMap<int[], String>();
        Document xmldoc;
        try{
            DocumentBuilder db = DocumentBuilderFactory.newInstance()
                .newDocumentBuilder();
            xmldoc = db.parse(file);
        }catch(Exception e){
            e.printStackTrace();
            return ret;
        }
        int maxx = Integer.MIN_VALUE, maxy = Integer.MIN_VALUE, minx = Integer.MAX_VALUE, miny = Integer.MAX_VALUE;
        NodeList trkseqs = xmldoc.getElementsByTagName("trkseg");
        for(int i = 0; i < trkseqs.getLength(); i++){
            Element trkseq = (Element)trkseqs.item(i);
            NodeList trkpts = trkseq.getElementsByTagName("trkpt");
            int length = trkpts.getLength();
            int[] path = new int[2 * length];
            for(int k = 0; k < length; k++){
                Element trkpt = (Element)trkpts.item(k);
                double lat = Double.parseDouble(trkpt.getAttribute("lat"));
                double lon = Double.parseDouble(trkpt.getAttribute("lon"));
                int[] trPoint = geo2pix(lat, lon);
                int j = 2 * k;
                int x = trPoint[0];
                int y = trPoint[1];
                path[j] = x;
                path[j + 1] = y;
                if(x > maxx){
                    maxx = x;
                }
                if(x < minx){
                    minx = x;
                }
                if(y > maxy){
                    maxy = y;
                }
                if(y < miny){
                    miny = y;
                }
            }
            ret.put(path, "trk");
        }
        NodeList rtes = xmldoc.getElementsByTagName("rte");
        for(int i = 0; i < rtes.getLength(); i++){
            Element rte = (Element)rtes.item(i);
            NodeList rtepts = rte.getElementsByTagName("rtept");
            int length = rtepts.getLength();
            int[] path = new int[2 * length];
            for(int k = 0; k < length; k++){
                Element rtept = (Element)rtepts.item(k);
                double lat = Double.parseDouble(rtept.getAttribute("lat"));
                double lon = Double.parseDouble(rtept.getAttribute("lon"));
                int[] trPoint = geo2pix(lat, lon);
                int j = 2 * k;
                int x = trPoint[0];
                int y = trPoint[1];
                path[j] = x;
                path[j + 1] = y;
                if(x > maxx){
                    maxx = x;
                }
                if(x < minx){
                    minx = x;
                }
                if(y > maxy){
                    maxy = y;
                }
                if(y < miny){
                    miny = y;
                }
            }
            ret.put(path, "rte");
        }
        NodeList wpts = xmldoc.getElementsByTagName("wpt");
        for(int k = 0; k < wpts.getLength(); k++){
            Element wpt = (Element)wpts.item(k);
            double lat = Double.parseDouble(wpt.getAttribute("lat"));
            double lon = Double.parseDouble(wpt.getAttribute("lon"));
            int[] trPoint = geo2pix(lat, lon);
            int x = trPoint[0];
            int y = trPoint[1];
            if(x > maxx){
                maxx = x;
            }
            if(x < minx){
                minx = x;
            }
            if(y > maxy){
                maxy = y;
            }
            if(y < miny){
                miny = y;
            }
            ret.put(trPoint, "wpt");
        }
        ret.put(new int[]{minx, miny, maxx, maxy}, "rec");
        return ret;
    }

    private Double[] transform(double lat, double lon){
        double h = 0;
        double fromLat_rad = lat * Math.PI / 180.;
        double fromLon_rad = lon * Math.PI / 180.;
        double SinfromLat = Math.sin(fromLat_rad);
        double SinfromLon = Math.sin(fromLon_rad);
        double CosfromLat = Math.cos(fromLat_rad);
        double CosfromLon = Math.cos(fromLon_rad);
        double sinsecond = Math.sin(1. / 3600. * Math.PI / 180.);

        double f = 1. / flatt;
        double da = 6378137. - a;
        double df = 1. / 298.257223563 - f;
        double e2 = 2. * f - f * f;
        double Rn = a / Math.pow(1. - e2 * SinfromLat * SinfromLat, 0.5);
        double Rm = a * (1. - e2)
            / Math.pow(1. - e2 * SinfromLat * SinfromLat, 1.5);

        double DeltaLat = (-1. * dx * SinfromLat * CosfromLon - dy * SinfromLat
            * SinfromLon + dz * CosfromLat + da * Rn * e2 * SinfromLat
            * CosfromLat / a + df * (Rm * 1. / (1. - f) + Rn * (1. - f))
            * SinfromLat * CosfromLat)
            / ((Rm + h) * sinsecond);
        double DeltaLon = (-1. * dx * SinfromLon + dy * CosfromLon)
            / ((Rn + h) * CosfromLat * sinsecond);

        return new Double[]{lon + DeltaLon / 3600., lat + DeltaLat / 3600.};
    }

    private void setDatum(String datum) throws IOException{
        //http://www.arsitech.com/mapping/geodetic_datum/
        this.datum = datum;
        if(datum == null){
            return;
        }
        if(datum.toLowerCase().equals("wgs 84")){
            this.datum = null;
            return;
        }
        InputStream dStream = FileLocator.openStream(Activator.getDefault()
            .getBundle(), new Path("res/datums.csv"), false);
        InputStream eStream = FileLocator.openStream(Activator.getDefault()
            .getBundle(), new Path("res/ellipsoids.csv"), false);
        Reader fr = new InputStreamReader(dStream);
        BufferedReader d = new BufferedReader(fr);
        String str;
        while((str = d.readLine()) != null){
            String[] strs = str.split(",");
            if(datum.toLowerCase().equals(strs[0].toLowerCase())){
                Reader r = new InputStreamReader(eStream);
                BufferedReader elipsoid = new BufferedReader(r);
                String s;
                while((s = elipsoid.readLine()) != null){
                    String[] ss = s.split(",");
                    if(strs[1].equals(ss[0])){
                        a = Double.parseDouble(ss[1]);
                        flatt = Double.parseDouble(ss[2]);
                        dx = Double.parseDouble(strs[2]);
                        dy = Double.parseDouble(strs[3]);
                        dz = Double.parseDouble(strs[4]);
                    }
                }
            }
        }
        dStream.close();
        eStream.close();
    }

    public Point getSize(){
        return size;
    }
}
